#include "prioqueue.hpp"
#include "critical.hpp"

using namespace qOS;

//==============================================================================
prioQueue::prioQueue(pq::queueStack_t *area, const size_t pq_size) noexcept
{
    critical::scope {
        stack = area;
        size = pq_size;
        for (size_t i = 0U;  i < size;  i++) {
            stack[i].pTask = nullptr;
        }
        index = -1;
    }
}
//==============================================================================
size_t prioQueue::count(void) const noexcept
{
    size_t retValue = 0U;

    if (hasElements()) {
        retValue = static_cast<size_t>(index) + 1U;
    }

    return retValue;
}
//==============================================================================
task* prioQueue::get(void) noexcept
{
    task *xTask = nullptr;

    if (hasElements()) {
        critical::scope {
            priority_t maxPriority;
            index_t indexTaskToExtract = 0U;

            maxPriority = stack[0].pTask->getPriority();
            for (index_t i = 1U;  (i < size) && (nullptr != stack[i].pTask);  i++) {
                const priority_t iPriorityValue = stack[i].pTask->getPriority();
                if (iPriorityValue > maxPriority) {
                    maxPriority = iPriorityValue;
                    indexTaskToExtract = i;
                }
            }
            data = stack[indexTaskToExtract].qData;
            xTask = stack[indexTaskToExtract].pTask;
            clearIndex(indexTaskToExtract);
        }
    }

    return xTask;
}
//==============================================================================
bool prioQueue::isTaskInside(const task &Task) const noexcept
{
    bool retValue = false;
    const base_t currentQueueIndex = index + 1;

    if (currentQueueIndex > 0) {
        critical::scope {
            for (base_t i = 0;  i < currentQueueIndex;  i++) {
                if (&Task == stack[i].pTask) {
                    retValue = true;
                    break;
                }
            }
        }
    }

    return retValue;
}
//==============================================================================
/*cstat -MISRAC++2008-7-1-2*/
bool prioQueue::insert(task &Task, void *pData) noexcept
{
    bool retValue = false;
    const base_t queueMaxIndex = static_cast<base_t>(size) - 1;

    if (index < queueMaxIndex) {
        pq::queueStack_t tmp;

        tmp.pTask = &Task;
        tmp.qData = pData;
        index = index + 1;  /* i++ndex  */
        stack[index] = tmp;  /*stack[i++ndex] = tmp*/
        retValue = true;
    }
    return retValue;
}
/*cstat +MISRAC++2008-7-1-2*/
//==============================================================================
void prioQueue::clearIndex(const index_t indexToClear) noexcept
{
    const base_t queueIndex = static_cast<base_t>(index);

    stack[indexToClear].pTask = nullptr;
    for (index_t j = indexToClear;  static_cast<base_t>(j) < queueIndex;  ++j) {
        stack[j] = stack[j + 1U];
    }
    index = index - 1;   /* --index */
}
//==============================================================================
void prioQueue::cleanUp(const task &Task) noexcept
{
    for (index_t i = 1U;  i < size;  i++) {
        if (stack[i].pTask == &Task) {
            clearIndex(i);
        }
    }
}
//==============================================================================
